#include <types.h>
#include <mmu.h>

static void mmuMapSectionTableRegion(Region *region);
static int mmuMapCoarseTableRegion (Region *region);
static void domainAccessSet(uint32_t value, uint32_t mask);
static void controlSet(uint32_t value, uint32_t mask);

/* Page Tables */
/* VADDRESS, PTADDRESS, MASTERPTADDRESS, PTTYPE, DOM */
//static Pagetable masterPT = {0x30700000, 0x30700000, 0x30700000, MASTER, 0};
/* Regions */
/* vAddress, pageSize, numPages, AP, CB, pAddress, PT */
// note: for now, no cached, otherwise the interrupt doesn't finish
//static Region masterRegion = {0x00000000, SECTIONPAGE, 4096, PTE_RWNA, PTE_cb, 0x00000000, &masterPT};

/* mmuInitPT
 * Initialize a page table by filling it with fault PTE */
int
mmuInitPT(Pagetable *pt) {
	uint32_t index; // number of entries to write
	uint32_t PTE, *PTEptr;
	PTEptr = (uint32_t *) pt->ptAddress;
	PTE = FAULT; // default PTE
	
	//cprintf("Pagetable type %d\n", pt->type);
	switch(pt->type)
	{
		case COARSE:
			index = NPTEENTRY/16;
			break;
		case MASTER:
			index = NPDEENTRY/16;
			break;
		default:
			//cprintf("Pagetable type incorrect: %d\n", pt->type);
			return -1;
	}
	
	asm volatile (
		"mov r0, %0;"
		"mov r1, %0;"
		::"r" (PTE):"r0","r1");
	
	for (; index > 0; index--)
	{
		asm volatile (
			"STMIA %0!, {r0-r1};" /* write 16 entries to table */
			"STMIA %0!, {r0-r1};"
			"STMIA %0!, {r0-r1};"
			"STMIA %0!, {r0-r1};"
			"STMIA %0!, {r0-r1};"
			"STMIA %0!, {r0-r1};"
			"STMIA %0!, {r0-r1};"
			"STMIA %0!, {r0-r1};"
			:"=r" (PTEptr):"r" (PTEptr):"r0","r1");
		
	}
	//cprintf("Pagetable written to 0x%08xd\n", PTEptr);
	//cprintf("Pagetable configured\n");
	return 0;
}

/* mmuMapRegion
 * Maps a memory region into L1 or L2 PT */
int 
mmuMapRegion(Region *region)
{
	switch (region->PT->type)
	{
		case MASTER: /* map section in L1 PT */
			mmuMapSectionTableRegion(region);
			break;
		case COARSE: /* map PTE to point to COARSE L2 PT */
			mmuMapCoarseTableRegion(region);
			break;
		default:
			//cprintf("UNKNOWN page table type\n");
			return -1;
	}
	return 0;
}

/* mmuMapSectionTableRegion
 * Maps a master region into L1 PT */
static void 
mmuMapSectionTableRegion(Region *region)
{
	int i;
	uint32_t *PTEptr, PTE;
		
	PTEptr = (uint32_t *)region->PT->ptAddress; /* base addr PT */
	PTEptr += region->vAddress >> PDXSHIFT; /* set to first PTE in region */
	PTEptr += region->numPages - 1; /* set to last PTE in region */
	
	PTE = region->pAddress & 0xfff00000; /* set physical address */
	PTE |= (region->AP & 0x3) << 10; /* set Access Permissions */
	PTE |= region->PT->dom << 5; /* set Domain for section */
	PTE |= (region->CB & 0x3) << 2; /* set Cache & WB attributes */
	PTE |= PTE_L1_SE; /* set as section entry */
	
	for (i =region->numPages - 1; i >= 0; i--) /* fill PTE in region */
	{
		*PTEptr-- = PTE + (i << PDXSHIFT); /* i = 1 MB section */
	}
}


/* mmuMapCoarseTableRegion
 * Maps a coarse region into L2 PT 
 * On exit, va mapped to pa */
static int 
mmuMapCoarseTableRegion (Region *region)
{
	int i,j;
	uint32_t *PTEptr, PTE;
	uint32_t tempAP = region->AP & 0x3;
	PTEptr = (uint32_t *)region->PT->ptAddress; /* base addr PT */
	switch (region->pageSize)
	{
		case LARGEPAGE:
			PTEptr += (region->vAddress & 0x000ff000) >> PTXSHIFT; /* 1st PTE */
			PTEptr += (region->numPages*16) - 1; /* region last PTE */
			PTE = region->pAddress & 0xffff0000; /* set physical address */
			PTE |= tempAP << 10; /* set Access Permissions subpage 3 */
			PTE |= tempAP << 8; /* subpage 2 */
			PTE |= tempAP << 6; /* subpage 1 */
			PTE |= tempAP << 4; /* subpage 0 */
			PTE |= (region->CB & 0x3) << 2; /* set cache & WB attributes */
			PTE |= 0x1; /* set as LARGE PAGE */
			/* fill in table entries for region */
			for (i = region->numPages-1; i >= 0; i--)
			{
				for (j = 15 ; j >= 0; j--)
					*PTEptr-- = PTE + (i << 16); /* i = 64 KB large page */
			}
			break;
		case SMALLPAGE:
			PTEptr += (region->vAddress & 0x000ff000) >> PTXSHIFT; /* first */
			PTEptr += (region->numPages - 1); /* last PTEptr */
			PTE = region->pAddress & 0xfffff000; /* set physical address */
			PTE |= tempAP << 10; /* set Access Permissions subpage 3 */
			PTE |= tempAP << 8; /* subpage 2 */
			PTE |= tempAP << 6; /* subpage 1 */
			PTE |= tempAP << 4; /* subpage 0 */
			PTE |= (region->CB & 0x3) << 2; /* set cache & WB attrib */
			PTE |= PTE_L2_SP; /* set as SMALL PAGE */
			/* fill in table entries for region */
			for (i = region->numPages - 1; i >= 0; i--)
			{
				*PTEptr-- = PTE + (i << PTXSHIFT); /* i = 4 KB small page */
			}
			break;
		default:
			//printf("mmuMapCoarseTableRegion: Incorrect page size\n");
			return -1;
	}
	return 0;
}

/* ttbSet
 * sets the TTB of the master L1 page table. equivalent of lcr3 */
inline void 
ttbSet(uint32_t ttb)
{
	ttb &= 0xffffc000;
	asm volatile ( 
		"MCR p15, 0, %0, c2, c0, 0" /* set translation table base */
		:: "r" (ttb)
	);
}

/* mmuAttachPT 
 * either activates an L1 master page table by placing its address
 * into the TTB in the CP15:c2:c0 register, or activates an L2 page 
 * table by placing its base address into an L1 master page table entry. */
int 
mmuAttachPT(Pagetable *pt) /* attach L2 PT to L1 master PT */
{
	uint32_t *ttb, PTE, offset;
	ttb = (uint32_t *) pt->masterPtAddress; /* read ttb from PT */
	offset = (pt->vAddress) >> PDXSHIFT; /* determine PTE from vAddress */
	switch (pt->type)
	{
		case MASTER:
			ttbSet((uint32_t) ttb);
			break;
		case COARSE:
			/* PTE = addr L2 PT | domain | COARSE PT type*/
			/* L2 PT aligned to 16kb */
			PTE = (pt->ptAddress & 0xfffffc00);
			PTE |= pt->dom << 5;
			PTE |= PTE_L1_CPT;
			ttb[offset] = PTE;
			break;
		default:
			//cprintf("UNKNOWN page table type\n");
			return -1;
	}
	return 0;
}

/* 16 domains */
static void 
domainAccessSet(uint32_t value, uint32_t mask)
{
	uint32_t c3format;
	asm volatile (
		"MRC p15, 0, %0, c3, c0, 0" /* read domain register */
		: "=r" (c3format)
	);
	c3format &= ~mask; /* clear bits that change */
	c3format |= value; /* set bits that change */
	asm volatile (
		"MCR p15, 0, %0, c3, c0, 0" /* write domain register */
		::"r" (c3format)
	);
}

/* controlSet
 * sets the control bits register in CP15:c1 */
static void 
controlSet(uint32_t value, uint32_t mask)
{
	uint32_t c1format;
	asm volatile (
		"MRC p15, 0, %0, c1, c0, 0" /* read control register */
		: "=r" (c1format)
	); 
	c1format &= ~mask; /* clear bits that change */
	c1format |= value; /* set bits that change */
	asm volatile (
		"MCR p15, 0, %0, c1, c0, 0" /* write control register */
		::"r" (c1format)
	); 
}

/* mmu_init - initialize the virtual memory management */
// Supposed already attached and initialized
/* 1. Initialize the page tables in main memory by filling them with FAULT entries.
 * 2. Fill in the page tables with translations that map regions to physical memory.
 * 3. Activate the page tables.
 * 4. Assign domain access rights.
 * 5. Enable the memory management unit and cache hardware.
 * */
void
mmu_init(void) {
	uint32_t enable, change;
	//~ /* Part 1 Initialize system (fixed) page tables */
	//~ mmuInitPT(&masterPT); /* init master L1 PT with FAULT PTE */
	
	//~ /* Part 2 filling page tables with translation & attribute data */
	//~ mmuMapRegion(&masterRegion); /* Map kernelRegion SystemPT */
	//~ mmuMapRegion(&coarseRegion); /* Map kernelRegion SystemPT */
	
	//~ /* Part 3 activating page tables */
	//~ mmuAttachPT(&masterPT); /* load L1 TTB to cp15:c2:c0 register */
	//~ mmuAttachPT(&coarsePT); /* load L1 TTB to cp15:c2:c0 register */
	
	/* Part 4 Set Domain Access */
	domainAccessSet(DOM3CLT, CHANGEALLDOM); /* set Domain Access */
	
	/* Part 5 Enable MMU, caches and write buffer */
	enable = ENABLEMMU | ENABLEICACHE | ENABLEDCACHE | ENABLEHIGHEVT;
	change = CHANGEMMU | CHANGEICACHE | CHANGEDCACHE | CHANGEHIGHEVT;
	
	/* enable cache and MMU */
	controlSet(enable, change); 
}
